//
//  Motion_Model.hpp
//  markerARLib
//
//  Created by fg on 6/20/16.
//  Copyright © 2016 CvidKal. All rights reserved.
//

#ifndef Motion_Model_hpp
#define Motion_Model_hpp


#include <array>
#include <queue>
#include "Basic Structure.h"
#include <Eigen/Dense>
#include <fstream>

namespace markerAR
{
    typedef Eigen::Vector3d translationVelocity,translationAcceleration;
    typedef Eigen::Vector3d angleVelocity,angleAcceleration;

typedef struct
{
    translationVelocity tv;
    translationAcceleration ta;
}TranslationMotion;

typedef struct
{
    angleVelocity rv;
    angleAcceleration ra;
}AngleMotion;

typedef struct RigidMotion
{
    double timestamp;
    Pose pose;
    TranslationMotion translationMotion;
    AngleMotion rotationMotion;
    
    friend std::ofstream& operator<<(std::ofstream& fout,RigidMotion& dt)
    {
        fout<<dt.timestamp<<" "<<dt.pose.t.transpose()<<" "<<dt.pose.EulerAngle().transpose();
        return fout;
    }
    
    friend std::ostream& operator<<(std::ostream& out, const RigidMotion& dt)
    {
        out<<"Translation:"<<endl;
        out<<"\tVelocity:\t"<<dt.translationMotion.tv<<endl;
        out<<"\tAcceleration:\t"<<dt.translationMotion.ta<<endl;
        out<<"Rotation:"<<endl;
        out<<"\tVelocity:\t"<<dt.rotationMotion.rv<<endl;
        out<<"\tAcceleration:\t"<<dt.rotationMotion.ra<<endl;
        return out;
    }

}RigidMotion;


class  Motion_Model
{
public:
    Motion_Model(){};
    Motion_Model(int windowSize):windowSize(windowSize){};
    RigidMotion addPose(const Pose&now,double timestamp);
private:
    std::queue<RigidMotion> motionWindow;
    int windowSize;
};

}



#endif /* Motion_Model_hpp */
